package ca.polymtl.rubikcube.util;


public final class Quaternion {
	/**
	 * The scalar part.
	 */
	private final double scalar;
	
	/**
	 * The vector part.
	 */
	private final Vect3f vector;
	
	/**
	 * Creates a quaternion of the form w + xi + yj + zk.
	 * @param w
	 * @param x
	 * @param y
	 * @param z
	 */
	public Quaternion(double w, double x, double y, double z) {
		this.scalar = w;
		vector = new Vect3f(x, y, z);
	}
	
	/**
	 * Creates a quaternion from a scalar and a vector.
	 * @param scalar
	 * @param vector
	 */
	public Quaternion(double scalar, Vect3f vector) {
		this.scalar = scalar;
		this.vector = vector;
	}
	
	/**
	 * Computes the square of the norm of the quaternion.
	 * @return The square of the norm of the quaternion.
	 */
	public double norm2() {
		return scalar * scalar + vector.norm2();
	}
	
	/**
	 * Computes the norm of the quaternion.
	 * @return The norm of the quaternion.
	 */
	public double norm() {
		return Math.sqrt(this.norm2());
	}
	
	/**
	 * Returns a normalized copy of this quaternion.
	 */
	public Quaternion normalized() {
		double n = norm();
		if (n != 0.0f) {
			return new Quaternion(this.scalar / n, this.vector.divide(n));
		} else {
			return this;
		}
	}
	
	/**
	 * Computes the sum of two quaternions.
	 * @param other
	 * @return A new quaternion that is the sum of both.
	 */
	public Quaternion add(Quaternion other) {
		return new Quaternion(this.scalar + other.scalar, this.vector.add(other.vector));
	}
	
	/**
	 * Computes the conjugate of the quaternion.
	 * @return The conjugate of the quaternion.
	 */
	public Quaternion conjugate() {
		return new Quaternion(scalar, -vector.x, -vector.y, -vector.z);
	}
	
	/**
	 * Multiplication of two quaternions (not commutative).
	 * @param rq
	 * @return The product of this quaternion and rq.
	 */
	public Quaternion product(Quaternion rq) {
		double w1 = scalar;
		double x1 = vector.x;
		double y1 = vector.y;
		double z1 = vector.z;
		double w2 = rq.scalar;
		double x2 = rq.vector.x;
		double y2 = rq.vector.y;
		double z2 = rq.vector.z;
		
		double w = w1*w2 - x1*x2 - y1*y2 - z1*z2;
		double x = w1*x2 + x1*w2 + y1*z2 - z1*y2;
		double y = w1*y2 - x1*z2 + y1*w2 + z1*x2;
		double z = w1*z2 + x1*y2 - y1*x2 + z1*w2;
		
		return new Quaternion(w, x, y, z);
	}
	
	public Quaternion inverse() {
		return new Quaternion(scalar, vector.inverse());
	}

	public double getScalar() {
		return scalar;
	}

	public Vect3f getVector() {
		return vector;
	}

	@Override
	public String toString() {
		return "Quaternion [scalar=" + scalar + ", vector=" + vector + "]";
	}	
}
